#include <ros/ros.h>

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <iostream>

#include "nxt_lab2/UpdatedMap.h"

/********************************** GLOBAL VARIABLES **********************************/

int wrapper_pxs = 60; // green + black
int entry_pxs = 50; // green

std::string cone_img_filename = "";
std::string nxt_img_filename = "";

/********************************** MAPPING FUNCTIONS **********************************/

/*
    Put img in grid cell

    Params:
      std::string filename: filename of img to be put in cell grid, for example nxt or cone
      cv::Mat & map_img: grid of cells
      int row: row defining cell
      int col: col defining cell
*/
void put_img(std::string filename, cv::Mat & map_img, int row, int col)
{
  cv::Mat img = cv::imread(filename);

  int border_pxs = (wrapper_pxs - entry_pxs) / 2; 
  int y = row * wrapper_pxs + border_pxs;
  int x = col * wrapper_pxs + border_pxs;
  cv::Mat roi(map_img, cv::Rect(x, y, entry_pxs, entry_pxs));
  
  resize(img, roi, cv::Size(entry_pxs, entry_pxs), 0, 0);
}

/*
    Put text in grid cell

    Params:
      std::string text: text to be written, for example START or END
      cv::Mat & map_img: grid of cells
      int row: row defining cell
      int col: col defining cell
*/
void put_text(std::string text, cv::Mat & map_img, int row, int col)
{
  int border_pxs = (wrapper_pxs - entry_pxs) / 2; 
  int y = row * wrapper_pxs + border_pxs;
  int x = col * wrapper_pxs + border_pxs;
  cv::Mat roi(map_img, cv::Rect(x, y, entry_pxs, entry_pxs));

  int fontFace = CV_STYLE_NORMAL;
  double fontScale = 0.5;
  int thickness = 1;
  int baseline = 0;
  cv::Size textSize = cv::getTextSize(text, fontFace, fontScale, thickness, &baseline);
  cv::Point textOrg((roi.cols - textSize.width) / 2, (roi.rows + textSize.height) / 2);

  cv::putText(roi, text, textOrg, fontFace, fontScale, cv::Scalar::all(0), thickness, 8);
}

/*
    Fill grid cell with color

    Params:
      cv::Mat & map_img: grid of cells
      int row: row defining cell
      int col: col defining cell
      cv::Scalar s: color used to fill, for example cv::Scalar(0,255,0) is green
*/
void fill_with_color(cv::Mat & map_img, int row, int col, cv::Scalar s)
{
  int border_pxs = (wrapper_pxs - entry_pxs) / 2; 
  int y = row * wrapper_pxs + border_pxs;
  int x = col * wrapper_pxs + border_pxs;
  cv::Mat roi(map_img, cv::Rect(x, y, entry_pxs, entry_pxs));
  roi = s;
}

/*
    Fill all cells with color

    Params:
      cv::Mat & map_img: grid of cells
      int rows: number of rows
      int cols: number of cols
*/
void init_map(cv::Mat & map_img, int rows, int cols)
{
  map_img.setTo(cv::Scalar(0,0,0)); // set all pixels to black
  for (int i = 0; i < rows; i++)
  {	
    for (int j = 0; j < cols; j++)
    {
      fill_with_color(map_img, i, j, cv::Scalar(0,255,0)); // green
    }
  }
}

/*
    Fill map with START, END, robot and obstacles

    Params:
      cv::Mat & map_img: grid of cells representing map
      int rows: number of rows
      int cols: number of cols
      std::vector<int> & world_map_vec: vector representing map to be drawn
      int nxt_pos: robot position
*/
void fill_map(cv::Mat & map_img, int rows, int cols, std::vector<int> & world_map_vec, int nxt_pos)
{
  int pos = 0;
  for (int i = 0; i < rows; i++)
  {	
    for (int j = 0; j < cols; j++)
    {
      if (pos == nxt_pos)
      	put_img(nxt_img_filename, map_img, i, j);
      else if (world_map_vec[pos] == -1)
        put_text("START", map_img, i, j);
      else if (world_map_vec[pos] == 1)
        put_text("END", map_img, i, j);
      else if (world_map_vec[pos] == -2)
        put_img(cone_img_filename, map_img, i, j);
      pos++;
    }
  }
  
}

/********************************** MAIN FUNCTION **********************************/

int main(int argc, char **argv) 
{

  ros::init(argc, argv, "map");

  ros::NodeHandle n;

  //ros::Rate r(0.1);
	
  std::cout << "Hello by map" << std::endl;

  n.getParam("/map/cone_img_filename", cone_img_filename);
  n.getParam("/map/nxt_img_filename", nxt_img_filename);

  ros::service::waitForService("send_updated_map");

  while(ros::ok())
  {
    //std::cout << "send_updated_map ready" << std::endl;
    ros::ServiceClient client = n.serviceClient<nxt_lab2::UpdatedMap>("send_updated_map");
    nxt_lab2::UpdatedMap srv;

    if (client.call(srv))
    {
      ROS_INFO("Succeeded in requesting updated_map");

      int rows = srv.response.rows;
      int cols = srv.response.cols;
      int nxt_pos = srv.response.nxt_pos;
      std::vector<int> world_map_vec = srv.response.updated_map;

      cv::Mat map_img(cv::Size(cols * wrapper_pxs, rows * wrapper_pxs), CV_8UC3);

      init_map(map_img, rows, cols);

      fill_map(map_img, rows, cols, world_map_vec, nxt_pos);

      cv::namedWindow("Map", 1);
      cv::imshow("Map", map_img);
      cv::waitKey(2000); // 5000 = 5 seconds - probabilmente da tenere molto basso, nell'ordine dei ms
      map_img.release();
      //r.sleep();
    }
    else
    {
      ROS_ERROR("Failed to call service send_updated_map");
      return 1;
    }
  }

  return 0;

}

/******************************** END MAIN FUNCTION ********************************/